#include "stm32f4xx.h"
#include "stm32f4xx_usart.h"
#include "bluetooth.h"
#include "diagnostic.h"


/* Variable definitions*/

#define UINT32 1

void SendDiagData()
{
	int8_t i;
	int8_t j;
	uint32_t *address;
	uint8_t size = 4;
	uint16_t tocnt = 0;

	USART_ITConfig(USART3,USART_IT_RXNE,DISABLE);

	for(j = 0; j < BT_rx_packnum + 1; j++)
	{
		if(rx_buffer[j].valid == 1)
		{
			address = (uint32_t*)rx_buffer[j].addr;
			if((rx_buffer[j].addr > 0x20000000U) && (rx_buffer[j].addr < 0x20016000U))
			{
				/* If read command received */
				if(rx_buffer[j].data[0] == 0x30U && rx_buffer[j].data[1] <= 4)
				{
					to_send = 1;
					/* Load start frame*/
					tx_buffer[0] = 0xABU;
					/* Load address to tx. buffer */
					for(i = 0; i < 4 ;i++)
					{
						tx_buffer[1 + i] = (uint8_t)(rx_buffer[j].addr >> i*8);
					}
					to_send += 4;

					/* Get data size to send */
					size = rx_buffer[j].data[1];
					to_send += size;

					/* Load tx. buffer with data */
					for(i = 0; i < size; i++)
					{
						tx_buffer[5 + i] = (uint8_t)((*address) >> i*8) & 0xFF;
					}

					/* Send data (Non-interrupt mode) */
					for(i = 0; i < to_send; i++)
					{
						USART_SendData(USART3,tx_buffer[i]);
						while(USART_GetFlagStatus(USART3,USART_FLAG_TXE)==0 || tocnt > 1000)
						{
							tocnt++;
						}
						/* TODO: fix interrupt mode, better timeout handling for non interrupt mode*/
						tocnt = 0;
					}
					rx_buffer[j].valid = 0;
					//rx_buffer[j].addr = 0x00000000U;
					to_send = 0;
				}
			}
		}

	}
	BT_rx_packnum = 0;
	USART_ITConfig(USART3,USART_IT_RXNE,ENABLE);
}
